#pragma once
#include <cmath>
#include <iostream>
#define StartStanleyNs namespace Stanley{
#define EndStanleyNs }

StartStanleyNs
constexpr double deg_to_rad = 3.14/180;
    struct StanleyPose
    {
        double x;
        double y;
        double theta;//unit is degree,not rad
    };
    
    class StanleyLateral
    {
        private:
            //controller parameter
            double wheel_base_ =2.91 ;
            double max_tire_angle = 40;
            //controller tunnable parameter
            double forward_gain_ =3;
            double backward_gain_ =3;
            
        public:
            //controller input
            StanleyPose current_pose_;//rear axle center
            StanleyPose ref_pose_;
            double speed_mps_;// is always positive or zero
            bool is_forward_ =true; // is driving direction forward ,1 true ,0 backward

            //controller output
            double tire_angle_; //unit is degree,left turn is positive

            //debug variable
            double delta_x_g_ ;//under global coordinate
            double delta_y_g_; //under global coordinate
            double delta_theta_;
            double lateral_error_; //from refrence point to car point in refrence coordinate y axix
            double longtitude_error_;//from refrence point to car point in refrence coordinate y axix
            bool step();
            StanleyLateral(double wheel_base =2.91,double forward_gain =5, double backward_gain=5);
            double wheelBase(){
                return wheel_base_;
            }
            void setWheelBase(double wheel_base){
                wheel_base_ = wheel_base;
            }
            double forwardGain(){
                return forward_gain_;
            }
            double setForwardGain(double forward_gain){
                forward_gain_ =forward_gain;
            }


    };
EndStanleyNs
